#include "bsp.h"
#include "math.h"

/***************************************************************************************
GPIO
***************************************************************************************/
struct BSP_GPIO bsp_test10_gpio[] = {
    {0, 0, 0, 0, 0},			     // 0
    {0, 0, 0, 0, 0},			     // 1
    {UIO_2_GPIO_Port, UIO_2_Pin, 0, 0, 0},   // 2
    {UIO_3_GPIO_Port, UIO_3_Pin, 0, 0, 0},   // 3
    {UIO_4_GPIO_Port, UIO_4_Pin, 0, 0, 0},   // 4
    {UIO_5_GPIO_Port, UIO_5_Pin, 0, 0, 0},   // 5
    {UIO_6_GPIO_Port, UIO_6_Pin, 0, 0, 0},   // 6
    {UIO_7_GPIO_Port, UIO_7_Pin, 0, 0, 0},   // 7
    {UIO_8_GPIO_Port, UIO_8_Pin, 0, 0, 0},   // 8
    {UIO_9_GPIO_Port, UIO_9_Pin, 0, 0, 0},   // 9
    {0, 0, 0, 0, 0},			     // 10
    {0, 0, 0, 0, 0},			     // 11
    {0, 0, 0, 0, 0},			     // 12
    {0, 0, 0, 0, 0},			     // 13
    {UIO_14_GPIO_Port, UIO_14_Pin, 0, 0, 0}, // 14
    {UIO_15_GPIO_Port, UIO_15_Pin, 0, 0, 0}, // 15
    {UIO_16_GPIO_Port, UIO_16_Pin, 0, 0, 0}, // 16
};

/**
 * @brief Get the Pin's STM32GPIO pin number from adu45pin
 *
 * @param luaPinNum adu-45 pin number
 * @return uint16_t stm32 gpio number
 */
uint16_t BSP_getSTM32PinNmb(uint8_t luaPinNum)
{
	return bsp_test10_gpio[luaPinNum].pin;
}

/**
 * @brief Get the Pin's STM32GPIO Port number from adu45pin
 *
 * @param luaPinNum adu-45 pin number
 * @return GPIO_TypeDef* stm32 port number
 */
GPIO_TypeDef *BSP_getSTM32PortNmb(uint8_t luaPinNum)
{
	// GPIO_TypeDef *port = NULL;
	return bsp_test10_gpio[luaPinNum].port;
}

int BSP_getKeyState(int keyNumber)
{
	GPIO_PinState keyState = -1;
	int ret = -1;

	switch (keyNumber) {
	case 0:
		keyState = HAL_GPIO_ReadPin(KEY0_GPIO_Port, KEY0_Pin);
		break;

	case 1:
		keyState = HAL_GPIO_ReadPin(KEY1_GPIO_Port, KEY1_Pin);
		break;

	default:
		break;
	}

	if (keyState == GPIO_PIN_RESET) {
		ret = 0;
	} else if (keyState == GPIO_PIN_SET) {
		ret = 1;
	}

	return ret;
}
/**
 * @brief pwm default initial, make pin state variable 0
 *
 * @return uint32_t
 */
void BSP_PwmDefaultInitial(void)
{
}

/***************************************************************************************
PWM
***************************************************************************************/
#define HAL_TIM_MODULE_ENABLED
#include "stm32f4xx_hal_tim.h"

extern TIM_HandleTypeDef htim4;
TIM_TypeDef *htimNmb;
uint32_t timChn;

TIM_TypeDef *_BSP_getSTM32TimNmb(uint8_t adu45PinNum);
uint32_t _BSP_getSTM32TimChn(uint8_t adu45PinNum);

void _BSP_TIM_Init(uint8_t adu45PinNum, uint32_t freq, uint8_t duty);
void _BSP_TIM_MspPostInit(uint8_t adu45PinNum);
void _BSP_TIM_Base_MspDeInit(uint8_t adu45PinNum);

TIM_TypeDef *_BSP_getSTM32TimNmb(uint8_t adu45PinNum)
{
	if (adu45PinNum == 14) {
		return TIM4;
	} else if (adu45PinNum == 15) {
		return TIM4;
	} else {
		return 0;
	}
}

uint32_t _BSP_getSTM32TimChn(uint8_t adu45PinNum)
{
	if (adu45PinNum == 14) { // PA6 TIM4_CH1
		return TIM_CHANNEL_1;
	} else if (adu45PinNum == 15) { // PA7 TIM4_CH2
		return TIM_CHANNEL_2;
	} else {
		return -1;
	}
}

void _BSP_TIM_Init(uint8_t adu45PinNum, uint32_t freq, uint8_t duty)
{
	/* USER CODE BEGIN TIM4_Init 0 */
	TIM4->EGR |= (1 << 0); // Bit 0: UG

	uint32_t channel = _BSP_getSTM32TimChn(adu45PinNum);
	/* USER CODE END TIM4_Init 0 */

	TIM_ClockConfigTypeDef sClockSourceConfig = {0};
	TIM_MasterConfigTypeDef sMasterConfig = {0};
	TIM_OC_InitTypeDef sConfigOC = {0};

	/* USER CODE BEGIN TIM4_Init 1 */

	/* USER CODE END TIM4_Init 1 */
	htim4.Instance = TIM4;
	htim4.Init.Prescaler = 8400000 / freq - 1;
	htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
	htim4.Init.Period = 10 - 1;
	htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
	htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
	if (HAL_TIM_Base_Init(&htim4) != HAL_OK) {
		Error_Handler();
	}
	sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
	if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK) {
		Error_Handler();
	}
	if (HAL_TIM_PWM_Init(&htim4) != HAL_OK) {
		Error_Handler();
	}
	sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
	sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
	if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) {
		Error_Handler();
	}
	sConfigOC.OCMode = TIM_OCMODE_PWM1;
	sConfigOC.Pulse = duty / 100;
	sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
	sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;

	if (channel == TIM_CHANNEL_1) {
		printf("[PWM] sel chn 1\r\n");
		if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) {
			Error_Handler();
		}
	} else if (channel == TIM_CHANNEL_2) {
		printf("[PWM] sel chn 2\r\n");
		if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) {
			Error_Handler();
		}
	} else {
		printf("[PWM channel select ERROR\r\n]");
	}
	/* USER CODE BEGIN TIM4_Init 2 */

	/* USER CODE END TIM4_Init 2 */
}

void _BSP_TIM_Init_Nocalc(uint8_t adu45PinNum, uint32_t preScale, uint32_t aar, uint32_t pulse)
{
	/* USER CODE BEGIN TIM4_Init 0 */
	TIM4->EGR |= (1 << 0); // Bit 0: UG

	uint32_t channel = _BSP_getSTM32TimChn(adu45PinNum);
	/* USER CODE END TIM4_Init 0 */

	TIM_ClockConfigTypeDef sClockSourceConfig = {0};
	TIM_MasterConfigTypeDef sMasterConfig = {0};
	TIM_OC_InitTypeDef sConfigOC = {0};

	/* USER CODE BEGIN TIM4_Init 1 */

	/* USER CODE END TIM4_Init 1 */
	htim4.Instance = TIM4;
	htim4.Init.Prescaler = preScale - 1;
	htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
	htim4.Init.Period = aar - 1;
	htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
	htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
	if (HAL_TIM_Base_Init(&htim4) != HAL_OK) {
		Error_Handler();
	}
	sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
	if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK) {
		Error_Handler();
	}
	if (HAL_TIM_PWM_Init(&htim4) != HAL_OK) {
		Error_Handler();
	}
	sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
	sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
	if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) {
		Error_Handler();
	}
	sConfigOC.OCMode = TIM_OCMODE_PWM1;
	sConfigOC.Pulse = pulse - 1;
	sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
	sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;

	if (channel == TIM_CHANNEL_1) {
		printf("[PWM] sel chn 1\r\n");
		if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) {
			Error_Handler();
		}
	} else if (channel == TIM_CHANNEL_2) {
		printf("[PWM] sel chn 2\r\n");
		if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) {
			Error_Handler();
		}
	} else {
		printf("[PWM channel select ERROR\r\n]");
	}
	/* USER CODE BEGIN TIM4_Init 2 */

	/* USER CODE END TIM4_Init 2 */
}

void _BSP_TIM_MspPostInit(uint8_t adu45PinNum)
{
	GPIO_InitTypeDef GPIO_InitStruct = {0};
	__HAL_RCC_GPIOB_CLK_ENABLE();

	uint32_t channel = _BSP_getSTM32TimChn(adu45PinNum);

	if (channel == TIM_CHANNEL_1) {
		printf("[PWM] sel chn 1\r\n");
		GPIO_InitStruct.Pin = GPIO_PIN_6;
		GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
		GPIO_InitStruct.Pull = GPIO_NOPULL;
		GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
		GPIO_InitStruct.Alternate = GPIO_AF2_TIM4;
		HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
	} else if (channel == TIM_CHANNEL_2) {
		printf("[PWM] sel chn 2\r\n");
		GPIO_InitStruct.Pin = GPIO_PIN_7;
		GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
		GPIO_InitStruct.Pull = GPIO_NOPULL;
		GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
		GPIO_InitStruct.Alternate = GPIO_AF2_TIM4;
		HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
	} else {
		printf("[PWM channel select ERROR\r\n]");
	}
}

void _BSP_TIM_Base_MspDeInit(uint8_t adu45PinNum)
{
	// TODO other TIM

	__HAL_RCC_TIM4_CLK_DISABLE();

	GPIO_InitTypeDef GPIO_InitStruct = {0};
	uint32_t channel = _BSP_getSTM32TimChn(adu45PinNum);

	if (channel == TIM_CHANNEL_1) {
		printf("[PWM] sel chn 1\r\n");
		GPIO_InitStruct.Pin = GPIO_PIN_6;
	} else if (channel == TIM_CHANNEL_2) {
		printf("[PWM] sel chn 2\r\n");
		GPIO_InitStruct.Pin = GPIO_PIN_7;
	} else {
		printf("[PWM channel select ERROR\r\n]");
	}

	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

	HAL_GPIO_WritePin(GPIOB, GPIO_InitStruct.Pin, GPIO_PIN_RESET);
	HAL_TIM_PWM_DeInit(&htim4);
}

struct timParaTypeDef {
	uint32_t preScale;
	uint32_t autoReload;
	uint32_t pulse;
};

float calculateAAR(float Fin, float preScale, float freq)
{
	return Fin / preScale / freq;
}

/**
 * @brief input Fin/PSC/AAR/PULSE and target freq/duty, output inaccuracy
 *
 */
int calculatePwmInaccuracy(float Fin, float freq, float duty, struct timParaTypeDef *i_pwmParas,
			   float *o_FreqOffset, float *o_DutyOffset)
{
	float realFreq;
	float realDuty;
	float freqOffset;
	float dutyOffset;

	// frequence offset = abs((RealFreq - TarFreq)/TarFreq)
	realFreq = Fin / i_pwmParas->preScale / (float)i_pwmParas->autoReload;
	freqOffset = fabs(realFreq - freq) / freq;

	// pulse offset = abs((RealPulse - TarPulse)/TarPulse)
	realDuty = 100 * (float)i_pwmParas->pulse / (float)i_pwmParas->autoReload;
	dutyOffset = fabs(realDuty - duty) / duty;

	*o_FreqOffset = freqOffset;
	*o_DutyOffset = dutyOffset;

	if (freqOffset < 0.03 && dutyOffset < 0.03) {
		return 0;
	} else {
		return -1;
	}
}

int calculatePwmInaccuracy_UnitTest(void)
{
	float Fin = 84000000.0;

	float freq;
	float duty;
	struct timParaTypeDef i_pwmParas;

	float freqOffset;
	float dutyOffset;
	int result;

	/* 1# */
	freq = 200000.0;
	duty = 4.0;
	i_pwmParas.preScale = 1;
	i_pwmParas.autoReload = 420;
	i_pwmParas.pulse = 16;
	result = calculatePwmInaccuracy(Fin, freq, duty, &i_pwmParas, &freqOffset, &dutyOffset);
	printf("freqoffset: %4f dudy offset %4f result %d\r\n", freqOffset, dutyOffset, result);

	/* 2# */
	freq = 200000.0;
	duty = 4.0;
	i_pwmParas.preScale = 3;
	i_pwmParas.autoReload = 140;
	i_pwmParas.pulse = 5;
	result = calculatePwmInaccuracy(Fin, freq, duty, &i_pwmParas, &freqOffset, &dutyOffset);
	printf("freqoffset: %4f dudy offset %4f result %d\r\n", freqOffset, dutyOffset, result);

	/* 2# */
	freq = 200000.0;
	duty = 40.0;
	i_pwmParas.preScale = 1;
	i_pwmParas.autoReload = 420;
	i_pwmParas.pulse = 168;
	result = calculatePwmInaccuracy(Fin, freq, duty, &i_pwmParas, &freqOffset, &dutyOffset);
	printf("freqoffset: %4f dudy offset %4f result %d\r\n", freqOffset, dutyOffset, result);

	return 0;
}

/**
 * @brief Caluclate PWM Reg Value
 * intput Bus frequence ,target frequence, target duty
 * output Autoreload/Prescaler/Pulse regesiter value
 * @param Fin APB bus frequence
 * @param freq target frequence
 * @param duty target duty
 * @param outAAR pointer, return AAR(auto reload register)
 * @param outPSC pointer, return PSC(prescaler register)
 * @param outPULSE pointer, return PULSE(pulse register)
 * @return find effective, 0 means nothing
 */
int findBestPwm(float Fin, float freq, float duty, uint16_t *outPSC, uint16_t *outAAR, uint16_t *outPULSE)
{
	// timer Frequence
	float Ftim;
	// devide input frequence to timer frequence
	int preScale = 0;
	float autoReload = 0;
	int autoReloadFloor = 0;
	float pulse = 0;
	int pulseFloor = 0;
	// count of auto reload reg and pulse reg are both in effective range( 1 <= r <= 65535)
	int effectiveCount = 0;

	// struct timParaTypeDef timParas[10];
	// int timParasIndex = 0;
	struct timParaTypeDef bestParas = {65535, 65535, 65535};

	float currentFreqOffset;
	float currentDutyOffset;

	float bestFreqOffset;
	float bestDutyOffset;

	// preScale range 0~65535
	for (preScale = 1; preScale <= 65535; preScale++) {

		// calculate timer frequence
		Ftim = Fin / (float)preScale;

		// calculate AAR, (how many timer cycle in one target cycle)
		autoReload = Ftim / (float)freq;

		if (autoReload > 65536) {
			// printf("auto reload register overflow\r\n");
			continue;
		} else if (autoReload >= 1) {
			// effective AAR

			// calculate pulse
			pulse = autoReload * (duty / 100.0);

			if (pulse > 65536) {
				// printf("pluse register overflow\r\n");
				continue;
			} else if (pulse >= 1) {
				// auto reload reg and pulse reg are both in effective range( 1 <= r <= 65535)
				effectiveCount++;

				if (effectiveCount > 10) {
					// find enough effective parameters(aar & pulse)
					break;
				} else {
					// check real freq and duty, if one of them > 2%, reject
					// calculate frequence and pulse offset
					autoReloadFloor = floor(autoReload);
					pulseFloor = floor(pulse);

					int result;

					struct timParaTypeDef currentTimPara = {preScale, autoReloadFloor, pulseFloor};
					result = calculatePwmInaccuracy(Fin, freq, duty, &currentTimPara, &currentFreqOffset, &currentDutyOffset);
					if (result != 0) {
						// offset are NOT in effective range, continue to search next time
						continue;
					} else {
						// if get very good offset, return directly!
						if (currentFreqOffset < 0.0001 && currentDutyOffset < 0.0001) {
							// end of search
							*outPSC = preScale;
							*outAAR = autoReloadFloor;
							*outPULSE = pulseFloor;
							return effectiveCount;
						}

						// initial best para, when prescalse = 1
						if (preScale == 1) {
							bestParas = currentTimPara;

							// The rest time, compare offset with best value;
						} else {
							calculatePwmInaccuracy(Fin, freq, duty, &bestParas, &bestFreqOffset, &bestDutyOffset);

							// find more accuracy parameter, update best paras;
							if (currentFreqOffset <= bestFreqOffset && currentDutyOffset <= bestFreqOffset) {
								bestParas = currentTimPara;
							}
						}
					}
				}

			} else { // pulse < 1
				printf("pulse register < 1, search stop\r\n");
				break;
			}

		} else { // autoReload < 1
			printf("auto reload register < 1, search stop\r\n");
			break;
		}
	}

	*outPSC = bestParas.preScale;
	*outAAR = bestParas.autoReload;
	*outPULSE = bestParas.pulse;

	return effectiveCount;
}

int findBestPwm_UnitTest(void)
{
	float Fin = 84000000;
	uint16_t outPSC;
	uint16_t outAAR;
	uint16_t outPULSE;
	float freq;
	float duty;
	int effectivCount;

	// #1
	freq = 200000;
	duty = 40;
	effectivCount = findBestPwm(Fin, freq, duty, &outPSC, &outAAR, &outPULSE);
	printf("Find:%d Tfreq:%f Tduty:%f PSC:%d AAR:%d PULSE:%d\r\n", effectivCount, freq, duty, outPSC, outAAR, outPULSE);

	// #2
	freq = 500000;
	duty = 40;
	effectivCount = findBestPwm(Fin, freq, duty, &outPSC, &outAAR, &outPULSE);
	printf("Find:%d Tfreq:%f Tduty:%f PSC:%d AAR:%d PULSE:%d\r\n", effectivCount, freq, duty, outPSC, outAAR, outPULSE);

	// #3
	freq = 500000;
	duty = 90;
	effectivCount = findBestPwm(Fin, freq, duty, &outPSC, &outAAR, &outPULSE);
	printf("Find:%d Tfreq:%f Tduty:%f PSC:%d AAR:%d PULSE:%d\r\n", effectivCount, freq, duty, outPSC, outAAR, outPULSE);

	return 0;
}

/**
 * @brief initial and start pwm output
 *
 * @param chn output channel, could be one from [2,3,4,5,14,15]
 * @param freq pwm frequency
 * @param pwm pwm duty cycle
 * @return uint32_t success = 0, failed = 1
 */
uint32_t BSP_PwmOn(uint8_t adu45PinNum, uint32_t freq, uint8_t duty)
{
	uint32_t channel = _BSP_getSTM32TimChn(adu45PinNum);
	TIM_OC_InitTypeDef sConfigOC;
	HAL_StatusTypeDef pwmStatus;

	float Fin = 84000000;
	uint16_t outPSC;
	uint16_t outAAR;
	uint16_t outPULSE;
	int effectivCount;

	effectivCount = findBestPwm(Fin, freq, duty, &outPSC, &outAAR, &outPULSE);

	if (effectivCount > 0) {
		printf("[PWM] find effective Timer setting SUCCESS\r\n");
		printf("[PWM] Real Reg Settings: PSC:%d AAR:%d PULSE:%d\r\n", outPSC, outAAR, outPULSE);

		//_BSP_TIM_Init(adu45PinNum, freq, duty);
		_BSP_TIM_Init_Nocalc(adu45PinNum, outPSC, outAAR, outPULSE);
		_BSP_TIM_MspPostInit(adu45PinNum);

		sConfigOC.OCMode = TIM_OCMODE_PWM1;
		sConfigOC.Pulse = outPULSE - 1;
		sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
		sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;

		pwmStatus = HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, channel);
		if (pwmStatus == HAL_OK) {
			printf("PWM PWM_ConfigChannel Success\r\n");
		} else {
			printf("PWM PWM_ConfigChannel Failed\r\n");
		}

		pwmStatus = HAL_TIM_PWM_Start(&htim4, channel);
		if (pwmStatus == HAL_OK) {
			printf("PWM PWM_Start Success\r\n");
		} else {
			printf("PWM PWM_Start Failed\r\n");
		}

	} else {
		printf("[PWM] find effective Timer setting FAILED\r\n");
	}

	return 0;
}

/**
 * @brief stop and deinitial pwm output
 *
 * @param chn output channel, could be one from [2,3,4,5,14,15]
 * @return uint32_t
 */
uint32_t BSP_PwmOff(uint8_t adu45PinNum)
{
	uint32_t channel = _BSP_getSTM32TimChn(adu45PinNum);

	HAL_StatusTypeDef pwmStatus = HAL_TIM_PWM_Stop(&htim4, channel);
	if (pwmStatus == HAL_OK) {
		printf("PWM Stop Success\r\n");
	} else {
		printf("PWM Stop Failed\r\n");
	}

	_BSP_TIM_Base_MspDeInit(adu45PinNum);

	return 0;
}
